#include "posfileinfo.h"
#include <QFile>
#include <QTextStream>
#include <QDebug>
#include <cmath>

PosFileInfo::PosFileInfo(void) :
    posCountSum(0),
    fixPosCount(0),
    floatPosCount(0),
    sigPosCount(0)
{

}

PosFileInfo::PosFileInfo(QString &fn) : fileName(fn),
    posCountSum(0),
    fixPosCount(0),
    floatPosCount(0),
    sigPosCount(0)

{

}

PosFileInfo::~PosFileInfo()
{

}

//read pos from filename
bool PosFileInfo::readPos()
{
    qint64 count = 0;
    QString lat, lon, alt, q;
    double s[3], r[3];
    QString respoString;
    bool havepos = false;
    if( pointMap.keys().length()>0 ) return true;

    if( QFile::exists(fileName) ){
        QFile file(fileName);

        if( file.open(QIODevice::ReadOnly) ){
            QTextStream in(&file);
            QStringList list;
            QString line;

            while (!in.atEnd()) {
              line = in.readLine();
              if( line.contains('%') )
              {
                  if(line.contains("latitude")){
                    list = line.split(" ", QString::SkipEmptyParts);
                    qDebug()<<list.join(" ");
                    havepos = true;
                  }

                  if( line.contains("ref pos") ){
                      list = line.split(":", QString::SkipEmptyParts);

                      list = list[1].split(" ", QString::SkipEmptyParts);
                      lat = list[0]; lon = list[1];
                      alt = list[2];
                      s[0] = lat.toDouble()*D2R;
                      s[1] = lon.toDouble()*D2R;
                      s[2] = alt.toDouble()*D2R;
                      pos2ecef(s, r);
                      pointMap["0-0-"+QString::number(0)+"-"+q] = QPointF(r[0], r[1]);
                      respoString = "Ref POS:  Lat= "+lat+"  Lon= "+lon + "  Alt="+alt+"\r\n";
                      havepos = true;
                      qDebug()<<list.join(" ");

                  }
                  continue;
              }

              if( havepos ){
                  list = line.split(" ", QString::SkipEmptyParts);
                  lat = list[2]; lon = list[3];
                  alt = list[4]; q = list[5];

                  /*
                  QStringList res;
                  res.append(lat);
                  res.append(lon);
                  res.append(alt);
                  res.append(q);
                  */

                  s[0] = lat.toDouble()*D2R;
                  s[1] = lon.toDouble()*D2R;
                  s[2] = alt.toDouble()*D2R;
                  pos2ecef(s, r);

                  ++count;
                  if(q=="1"){
                      ++fixPosCount;
                  }else if(q=="2"){
                      ++floatPosCount;
                  }else{
                      ++sigPosCount;
                  }
                  ++posCountSum;
                  //qDebug()<<list[0]+"-"+list[1]+"-"+QString::number(count)+"-"+q;
                  pointMap[list[0]+"-"+list[1]+"-"+QString::number(count)+"-"+q] = QPointF(r[0], r[1]);
              }
            }
            file.close();
        }
    }

    float fixrate = fixPosCount*1.0/posCountSum;
    float floatRate = floatPosCount*1.0/posCountSum;

    respoString += "Pos Count = "+QString::number(posCountSum)+"\r\n";
    respoString += "Fix Count = "+QString::number(fixPosCount)+"(" + QString::number(fixrate*100, 'f', 1)+ "%) "+"\r\n";
    respoString += "Float Count = "+QString::number(floatPosCount)+"(" + QString::number(floatRate*100, 'f', 1)+ "%) "+"\r\n";

    infoString = respoString;

    if( pointMap.keys().length()>0 ) return true;

    return false;
}

QString &PosFileInfo::getPosMsg()
{
    return infoString;
}

QMap<QString, QPointF> &PosFileInfo::getPoints()
{
    return pointMap;
}

/* transform geodetic to ecef position -----------------------------------------
* transform geodetic position to ecef position
* args   : double *pos      I   geodetic position {lat,lon,h} (rad,m)
*          double *r        O   ecef position {x,y,z} (m)
* return : none
* notes  : WGS84, ellipsoidal height
*-----------------------------------------------------------------------------*/
void PosFileInfo::pos2ecef(const double *pos, double *r)
{
    double sinp=sin(pos[0]),cosp=cos(pos[0]),sinl=sin(pos[1]),cosl=cos(pos[1]);
    double e2=FE_WGS84*(2.0-FE_WGS84),v=RE_WGS84/sqrt(1.0-e2*sinp*sinp);

    r[0]=(v+pos[2])*cosp*cosl;
    r[1]=(v+pos[2])*cosp*sinl;
    r[2]=(v*(1.0-e2)+pos[2])*sinp;
}
